#include "aos/kernel.h"
#include <aos/mbmaster.h>
#include "modbus.h"
#include "aos/cli.h"

#define MOTO_UP_A  (6)
#define MOTO_UP_B  (7)
#define MOTO_LIFT  (8)

#define SAR_ON        (0x60)
#define STB           (0x120)
#define HOME          (0x122)
#define H_STOP        (0x123)
#define S_STOP        (0x124)

#define STA_SAR_ON    (0x20)
#define STA_STB       (0x130)
#define STA_HOME      (0x132)
#define STA_H_STOP    (0x133)
#define STA_S_STOP    (0x134)
#define BUSY          (0x140)
#define HOME_CMP      (0x141)

#define BLOCK_ID      (0x4414)
#define BLOCK_ACTUALNO   (0x4415)
#define BLOCK_NOW_ID  (0x4416)

#define BLOCK_VEL0     (0x4600)
#define BLOCK_VEL1     (0x4601)
#define BLOCK_VEL2     (0x4602)
#define BLOCK_VEL3     (0x4603)
#define BLOCK_VEL4     (0x4604)
#define BLOCK_VEL5     (0x4605)
#define BLOCK_VEL6     (0x4606)
#define BLOCK_ACC0     (0x4610)
#define BLOCK_ACC1     (0x4611)
#define BLOCK_ACC2     (0x4612)
#define BLOCK_ACC3     (0x4613)
#define BLOCK_ACC4     (0x4614)
#define BLOCK_ACC5     (0x4615)
#define BLOCK_DEC0     (0x4620)
#define BLOCK_DEC1     (0x4621)
#define BLOCK_DEC2     (0x4622)
#define BLOCK_DEC3     (0x4623)
#define BLOCK_DEC4     (0x4624)
#define BLOCK_DEC5     (0x4625)

#define BLOCK_BASE      (0x4800)
#define BLOCK_CMD_BASE  (BLOCK_BASE)
#define BLOCK_DATA_BASE (BLOCK_BASE+2)

#define MOTO_ON       (0xFF00)
#define MOTO_OFF      (0x0000)

#define JUMP_VAL             (0x09000000)   
#define jump_block(val)      (JUMP_VAL & (val << 10))
#define infinite_direct(val, direct) (direct?((0x1 << 10) | val) : ((~(0x1 << 10)) & val))

#define BLOCK_HOME        (0)
#define BLOCK_INFINITE    (1)
#define BLOCK_JUMP        (2)
#define BLOCK_DOWN        (3)
#define BLOCK_DOWN_NOW    (4)
#define BLOCK_UPDATE      (5)
#define BLOCK_REL_10K     (6)
#define BLOCK_REL_20K     (7)
#define BLOCK_REL_50K     (8)
#define BLOCK_REL_100K    (9)
#define BLOCK_REL_500K    (10)
#define BLOCK_REL_1KK     (11)
#define BLOCK_REL_OTH     (12)
#define BLOCK_ABS_10K     (13)
#define BLOCK_ABS_20K     (14)
#define BLOCK_ABS_50K     (15)
#define BLOCK_ABS_100K    (16)
#define BLOCK_ABS_1KK     (17)
#define BLOCK_ABS_OTH     (18)
#define BLOCK_ABS_LIFT    (19)

#define sar_on(addr, on)   set_coil(addr, SAR_ON, on)
#define stb(addr, on)      set_coil(addr, STB, on)
#define home(addr, on)     set_coil(addr, HOME, on)
#define h_stop(addr, on)   set_coil(addr, H_STOP, on)
#define s_stop(addr, on)   set_coil(addr, S_STOP, on)

#define sta_sar_on(addr)   get_coil(addr, STA_SAR_ON)
#define sta_stb(addr)      get_coil(addr, STA_STB)
#define sta_home(addr)     get_coil(addr, STA_HOME)
#define sta_h_stop(addr)   get_coil(addr, STA_H_STOP)
#define sta_s_stop(addr)   get_coil(addr, STA_S_STOP)
#define sta_home_cmp(addr) get_coil(addr, HOME_CMP)
#define sta_busy(addr)     get_coil(addr, BUSY)

#define block_no(addr, no)           set_reg(addr, BLOCK_ID, no)
#define block_actual_no(addr)    get_reg(addr, BLOCK_ACTUALNO)
#define block_now_no(addr)       get_reg(addr, BLOCK_NOW_ID)
#define block_cmd_addr(no)      (BLOCK_CMD_BASE + no * 4)
#define block_data_addr(no)    (BLOCK_DATA_BASE + no * 4)
#define block_set_vel
#define block_set_acc
static int8_t get_coil(uint16_t addr, uint16_t reg);
static uint16_t set_coil(uint16_t addr, uint16_t coil, uint16_t val);

void error_handle(uint16_t val)
{
	switch(val){
		case 0:
			printf("function error\r\n");
			break;
		case 1:
			printf("reg error\r\n");
			break;
		case 2:
			printf("data error\r\n");
			break;
		case 3:
			printf("res error\r\n");
			break;
		default:
			break;
	}
	
}

static int8_t get_coil(uint16_t addr, uint16_t reg)
{
	uint8_t buf[10];
	uint32_t len = 0;
	md_dev_t md_rd_buf;

	md_rd_buf.rw_bit = MD_COIL_G;
	md_rd_buf.dev_id = addr;
	md_rd_buf.reg_id = reg;
	md_rd_buf.rwlen = 1;
	md_rd_buf.reslen = &len;
	md_rd_buf.resbuf = buf;
	if (!modbus_status()){
		if (modbus_opt(&md_rd_buf) < 0){
			if (1 == len){
				error_handle(buf[0]);
			}
			return -1;
		}
		printf("get_coil %x\r\n", buf[0]);
		return buf[0];
	}
	return -1;
}

static uint16_t set_coil(uint16_t addr, uint16_t coil, uint16_t val)
{
	uint8_t buf[10];
	uint32_t len = 0;
	uint16_t num = 0;
	md_dev_t md_rd_buf;

	md_rd_buf.rw_bit = MD_COIL_S;
	md_rd_buf.dev_id = addr;
	md_rd_buf.reg_id = coil;
	md_rd_buf.rwlen = val;
	md_rd_buf.reslen = &len;
	md_rd_buf.resbuf = buf;
	printf("set_coil%x %x %d\r\n",addr, coil, val);
	if (!modbus_status()){
		if (modbus_opt(&md_rd_buf) < 0){
			printf("set_coil opt error\n");
			return -1;
		}

		if (1 == len){
			error_handle(buf[0]);
			return -1;
		}
		num = (buf[0] << 8) | buf[1];
		return num;
	}
	return -1;
}

static int32_t get_block_data(uint16_t addr, uint16_t reg)
{
	uint8_t buf[10];
	uint32_t len = 0;
	md_dev_t md_rd_buf;
	int32_t val = 0;

	md_rd_buf.rw_bit = MD_READ_D;
	md_rd_buf.dev_id = addr;
	md_rd_buf.reg_id = reg;
	md_rd_buf.rwlen = 2;
	md_rd_buf.reslen = &len;
	md_rd_buf.resbuf = buf;
	if (!modbus_status()){
		if (modbus_opt(&md_rd_buf) < 0){
			return -1;
		}

		if (1 == len){
			error_handle(buf[0]);
			return -1;
		}

		val = ((buf[0]<<8) | buf[1]) | (((buf[2]<<8) | buf[3]) << 16); 
		printf("get_block_data %d\r\n", val);
		return val;
	}
	return -1;
}

static int32_t valbuf;
static int32_t set_block_data(uint16_t addr, uint16_t reg, int32_t val)
{
	uint8_t buf[10] = {0};
	uint32_t len = 0;
	uint16_t num = 0;
	md_dev_t md_rd_buf;
	valbuf = val;

	md_rd_buf.rw_bit = MD_WRITE_D;
	md_rd_buf.dev_id = addr;
	md_rd_buf.reg_id = reg;
	md_rd_buf.rwlen = 2;
	md_rd_buf.wbuf = &valbuf;
	md_rd_buf.reslen = &len;
	md_rd_buf.resbuf = buf;

	if (!modbus_status()){
		if (modbus_opt(&md_rd_buf) < 0){
			return -1;
		}

		if (1 == len){
			error_handle(buf[0]);
			return -1;
		}
		
		num = *(uint16_t *)buf;
		return num;
	}
	return -1;
}


int32_t slow_down(uint16_t addr)
{
	if (sta_busy(addr)){
		return -1;
	}

	if (0 == sta_sar_on(addr)){
		if (sar_on(addr, MOTO_ON) < 0){
			return -1;
		}
	}

	stb(addr, MOTO_OFF);
	block_no(addr, BLOCK_DOWN);
	stb(addr, MOTO_ON);
	
	return -1;
}

int32_t slow_down_now(uint16_t addr)
{
	sar_on(addr, MOTO_OFF);
	sar_on(addr, MOTO_ON);

	return 0;
	if (sta_busy(addr)){
		return -1;
	}

	if (0 == sta_sar_on(addr)){
		if (sar_on(addr, MOTO_ON) < 0){
			return -1;
		}
	}

	stb(addr, MOTO_OFF);
	block_no(addr, BLOCK_DOWN_NOW);
	stb(addr, MOTO_ON);
	
	return -1;
}


int32_t abs_local(uint16_t addr, int32_t val)
{
	if (sta_busy(addr)){
		return -1;
	}

	if (0 == sta_sar_on(addr)){
		if (sar_on(addr, MOTO_ON) < 0){
			return -1;
		}
	}

	set_block_data(addr, block_data_addr(BLOCK_ABS_OTH), val);

	stb(addr, MOTO_OFF);
	block_no(addr, BLOCK_ABS_OTH);
	stb(addr, MOTO_ON);
	
	return -1;
}

int32_t rel_local(uint16_t addr, int32_t val)
{
	if (sta_busy(addr)){
		return -1;
	}

	if (0 == sta_sar_on(addr)){
		if (sar_on(addr, MOTO_ON) < 0){
			return -1;
		}
	}

	set_block_data(addr, block_data_addr(BLOCK_REL_OTH), val);

	stb(addr, MOTO_OFF);
	block_no(addr, BLOCK_REL_OTH);
	stb(addr, MOTO_ON);
	return 0;
}

int32_t infinite_run(uint16_t addr, int val)
{
	int32_t direct = 0;
	if (sta_busy(addr)){
		return -1;
	}

	if (0 == sta_sar_on(addr)){
		if (sar_on(addr, MOTO_ON) < 0){
			return -1;
		}
	}

	direct = get_block_data(addr, block_cmd_addr(BLOCK_INFINITE));
	if (direct < 0){
		return -1;
	}

	direct = infinite_direct(direct, val);
	direct = set_block_data(addr, block_cmd_addr(BLOCK_INFINITE), direct);
	if (direct < 0){
		return -1;
	}
	
	stb(addr, MOTO_OFF);
	block_no(addr, BLOCK_INFINITE);
	stb(addr, MOTO_ON);
	
	return 0;
}

int32_t jump_b(uint16_t addr, uint16_t b)
{
	if (sta_busy(addr)){
		return -1;
   	}

	if (0 == sta_sar_on(addr)){
		if (sar_on(addr, MOTO_ON) < 0){
			return -1;
		}
	}

	set_block_data(addr, block_cmd_addr(BLOCK_JUMP), jump_block(b));

	stb(addr, MOTO_OFF);
	block_no(addr, BLOCK_INFINITE);
	stb(addr, MOTO_ON);
	
	return -1;
}

int upping(int val)
{
	if ((sta_busy(MOTO_UP_A) != 0) || (sta_busy(MOTO_UP_A) != 0)){
		return -1;
   	}

	if (val == 750 || val == 0){
		int32_t direct = 0;
		direct = get_block_data(MOTO_UP_A, block_cmd_addr(BLOCK_INFINITE));
		if (direct < 0){
			return -1;
		}

		direct = infinite_direct(direct, val);
		set_block_data(MOTO_UP_A, block_cmd_addr(BLOCK_INFINITE), direct);
		set_block_data(MOTO_UP_B, block_cmd_addr(BLOCK_INFINITE), direct);
		
		stb(MOTO_UP_A, MOTO_OFF);
		stb(MOTO_UP_B, MOTO_OFF);
		block_no(MOTO_UP_A, BLOCK_INFINITE);
		block_no(MOTO_UP_B, BLOCK_INFINITE);
		stb(MOTO_UP_A, MOTO_ON);
		stb(MOTO_UP_B, MOTO_ON);
	} else {
		set_block_data(MOTO_UP_A, block_data_addr(BLOCK_REL_OTH), val);
		set_block_data(MOTO_UP_B, block_data_addr(BLOCK_REL_OTH), val);
		stb(MOTO_UP_A, MOTO_OFF);
		stb(MOTO_UP_B, MOTO_OFF);
		block_no(MOTO_UP_A, BLOCK_REL_OTH);
		block_no(MOTO_UP_B, BLOCK_REL_OTH);
		stb(MOTO_UP_A, MOTO_ON);
		stb(MOTO_UP_B, MOTO_ON);
	}
	return 0;
}

int liftting(int val)
{
	if (sta_busy(MOTO_LIFT) != 0){
		return -1;
   	}

	if (val == 1 || val == 0){
		if (infinite_run(MOTO_LIFT, val) < 0){
			sar_on(MOTO_LIFT, MOTO_OFF);
			return -1;
		} else if (infinite_run(MOTO_LIFT, val) < 0){
			sar_on(MOTO_LIFT, MOTO_OFF);
			return -1;
		}
	} else {
		if (rel_local(MOTO_LIFT, val) < 0){
			sar_on(MOTO_LIFT, MOTO_OFF);
			return -1;
		}
	}
	return 0;
}


void stop_up(void)
{
	sar_on(MOTO_UP_A, MOTO_OFF);
	sar_on(MOTO_UP_B, MOTO_OFF);

	sar_on(MOTO_UP_A, MOTO_ON);
	sar_on(MOTO_UP_B, MOTO_ON);
}


static void psc_cmd_routine(char *buf, int len, int argc, char **argv)
{
	int ret = 0;
	int arg1 = strtoul(argv[2], NULL, 10);
    int arg2 = strtoul(argv[3], NULL, 10);
	int arg3 = strtoul(argv[4], NULL, 16);
	
	switch (*(argv[1]))
	{
	case 'i':	   
       infinite_run(arg1, arg2);
	   aos_cli_printf("infinite_run dev [%d] direct %d\r\n", arg1, arg2);
	   break;
	case 'j':	   
       jump_b(arg1, arg2);
	   aos_cli_printf("dev [%d]  jump b[%d]\r\n", arg1, arg2);
	   break;
	case 'a':	   
       abs_local(arg1, arg2);
	   aos_cli_printf("dev [%d]  val[%d]\r\n", arg1, arg2);
	   break;
	case 'r':	   
       rel_local(arg1, arg2);
	   aos_cli_printf("dev [%d]  val[%d]\r\n", arg1, arg2);
	   break;
	case 'd':	   
       slow_down_now(arg1);
	   aos_cli_printf("dev [%d]  slow down\r\n", arg1);
	   break;
	case 'u':
		arg1 = arg1 * 750;
       upping(arg1);
	   aos_cli_printf("dev  upping[%d]\r\n", arg1);
	   break;
	case 'l':
		arg1 = arg1 * 3889;
		liftting(arg1);
		aos_cli_printf("ddev liftting[%d]\r\n", arg1);
		break;
	case 's':	   
       stop_up();
	   aos_cli_printf("stop up\r\n");
	   break;
	case 'p':
	   break;

	default:
	   aos_cli_printf("moto Unknown cs %d\r\n", *(argv[1]));
	   break;
	}
}


static struct cli_command psc_cmd[] = {
    { "psc","control modbusr", psc_cmd_routine },
};

void panasonic_init()
{
	sar_on(MOTO_UP_A, MOTO_ON);
	sar_on(MOTO_UP_B, MOTO_ON);
	sar_on(MOTO_LIFT, MOTO_ON);
	aos_cli_register_commands(&psc_cmd[0], sizeof(psc_cmd) / sizeof(struct cli_command));
}


